#ifndef _INFORMED_RRT_H
#define _INFORMED_RRT_H

#include "KDTree.hpp"
#include "KDNode.hpp"
#include <vector>
#include <algorithm>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_global_planner.h>
#include <geometry_msgs/PoseStamped.h>

using std::vector;
using KDNodePtr = std::shared_ptr<KDNode>;
using point_t = vector<double>;

namespace global_planner{

class informedRRT{
public:
    informedRRT() = default;
    int nx_;
    int ny_;
    void setSize(int nx, int ny){
        nx_ = nx;
        ny_ = ny;
    }
    int getNodeIndex(const KDNodePtr& n){
        return n->point[X] + n->point[Y] * nx_;
    }
    bool search(unsigned char* costs,
                const KDNodePtr& start,
                const KDNodePtr& goal,
                vector<KDNodePtr>& path);
    static double GetEuclideanDistance(const KDNodePtr& p1, const KDNodePtr& p2);
    bool isCollision(unsigned char* costs, const KDNodePtr& p1, const KDNodePtr& p2, double maxDist);
    bool isCollision(unsigned char* costs, const KDNodePtr& p1, const KDNodePtr& p2);
    class Hyperellipsoid
    {
    public:
        double c_max = 0;
        double c_min = 0;

        double rotationMatrix[2][2];

        KDNodePtr centre;

        Hyperellipsoid() = default;
        Hyperellipsoid(const KDNodePtr& origin, const KDNodePtr& goal)
        {
            c_min = GetEuclideanDistance(goal, origin);
            centre = std::make_shared<KDNode>((origin->point[X] + goal->point[X]) / 2, (origin->point[Y] + goal->point[Y]) / 2);
            double theta = -atan2(goal->point[Y] - origin->point[Y], goal->point[X] - origin->point[X]);
            rotationMatrix[0][0] = cos(theta);
            rotationMatrix[0][1] = sin(theta);
            rotationMatrix[1][0] = -sin(theta);
            rotationMatrix[1][1] = cos(theta);
        }

    };

    KDNodePtr RotationToWorldFrame(const KDNodePtr& sample, const Hyperellipsoid& hyperellipsoid);
    KDNodePtr GetSample(unsigned char* costs, const Hyperellipsoid& hyperellipsoid, double c_max);

};

};

#endif